﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using NIP.ControllerInterface ;
using NIP.ControlLogic;

namespace NIP
{
    public partial class MainWindow : Form
    {
        VisionSharp.Potraitor portraitor;
        public MainWindow()
        {
            InitializeComponent();
            portraitor = new VisionSharp.Potraitor();
            portraitor.ContourReadyDrawEvent += new EventHandler<VisionSharp.DrawEventArgs>(DrawEventHandler);
        }

        public event EventHandler<NIP.DynamicControl.RobotControlEventArgs> ControlEvent;
        public event EventHandler<EventArgs> getCurrentEvent;
        public event EventHandler<NIP.VisionSharp.DrawEventArgs> drawEvent;
        public void DrawEventHandler(object sender, NIP.VisionSharp.DrawEventArgs args)
        {
            if (drawEvent != null)
            {
                drawEvent(this, args);
            }
        }
        private void moveTo_Click(object sender, EventArgs e)
        {
            DynamicControl.RobotControlEventArgs controlCommand = new DynamicControl.RobotControlEventArgs();
            controlCommand.X = Convert.ToDouble(textBox1.Text);
            controlCommand.Y = Convert.ToDouble(textBox2.Text);
            controlCommand.Z = Convert.ToDouble(textBox3.Text);
            controlCommand.gPitch = Convert.ToDouble(textBox4.Text);
            controlCommand.gRoll = Convert.ToDouble(textBox5.Text);
            controlCommand.gOffset = Convert.ToDouble(textBox6.Text);
            controlCommand.command = DynamicControl.ControlCommands.MOVE_TO;
            if (ControlEvent != null)
            {
                ControlEvent(new object(), controlCommand);
            }
        }
        
        private void lookAt_Click(object sender, EventArgs e)
        {
            DynamicControl.RobotControlEventArgs controlCommand = new DynamicControl.RobotControlEventArgs();
            controlCommand.X = Convert.ToDouble(textBox1.Text);
            controlCommand.Y = Convert.ToDouble(textBox2.Text);
            controlCommand.Z = Convert.ToDouble(textBox3.Text);
            controlCommand.gPitch = Convert.ToDouble(textBox4.Text);
            controlCommand.gRoll = Convert.ToDouble(textBox5.Text);
            controlCommand.gOffset = Convert.ToDouble(textBox6.Text);
            controlCommand.command = DynamicControl.ControlCommands.LOOK_AT ;
            if (ControlEvent != null)
            {
                ControlEvent(new object(), controlCommand);
            }
        }
        
        private void pointTo_Click(object sender, EventArgs e)
        {
            DynamicControl.RobotControlEventArgs controlCommand = new DynamicControl.RobotControlEventArgs();
            controlCommand.X = Convert.ToDouble(textBox1.Text);
            controlCommand.Y = Convert.ToDouble(textBox2.Text);
            controlCommand.Z = Convert.ToDouble(textBox3.Text);
            controlCommand.gPitch = Convert.ToDouble(textBox4.Text);
            controlCommand.gRoll = Convert.ToDouble(textBox5.Text);
            controlCommand.gOffset = Convert.ToDouble(textBox6.Text);
            controlCommand.command = DynamicControl.ControlCommands.POINT_TO ;
            if (ControlEvent != null)
            {
                ControlEvent(new object(), controlCommand);
            }
        }

        private void button4_Click(object sender, EventArgs e)
        {
            getCurrentEvent(this, new EventArgs());

        }

        private void testPID(object sender, EventArgs e)
        {
            DynamicControl.RobotControlEventArgs controlCommand = new DynamicControl.RobotControlEventArgs();
            controlCommand.command = DynamicControl.ControlCommands.TEST_PID;
            if (ControlEvent != null)
            {
                ControlEvent(new object(), controlCommand);
            }

        }

        private void home_Click(object sender, EventArgs e)
        {
            DynamicControl.RobotControlEventArgs controlCommand = new DynamicControl.RobotControlEventArgs();
            controlCommand.command = DynamicControl.ControlCommands.HOME;
            if (ControlEvent != null)
            {
                ControlEvent(new object(), controlCommand);
            }
        }

        private void gripper_Click(object sender, EventArgs e)
        {
            DynamicControl.RobotControlEventArgs controlCommand = new DynamicControl.RobotControlEventArgs();
            controlCommand.command = DynamicControl.ControlCommands.GRIP;
            if (ControlEvent != null)
            {
                ControlEvent(new object(), controlCommand);
            }

        }
        

        private void textBox1_TextChanged(object sender, EventArgs e)
        {

        }

        private void textBox2_TextChanged(object sender, EventArgs e)
        {

        }

        private void textBox3_TextChanged(object sender, EventArgs e)
        {

        }

        private void label1_Click(object sender, EventArgs e)
        {

        }

        private void enablePID_Click(object sender, EventArgs e)
        {
            DynamicControl.RobotControlEventArgs controlCommand = new DynamicControl.RobotControlEventArgs();
            controlCommand.command = DynamicControl.ControlCommands.ENABLE_PID ;
            if (ControlEvent != null)
            {
                ControlEvent(new object(), controlCommand);
            }


        }

        private void SetPIDJ4_Click(object sender, EventArgs e)
        {
            DynamicControl.RobotControlEventArgs controlCommand = new DynamicControl.RobotControlEventArgs();
            int holdPWM = Convert.ToInt32(PWMJ4.Text);
            double[] Angle = new double[USBC.TotalAxes] { Convert.ToDouble(AngleJ0.Text), Convert.ToDouble(AngleJ1.Text), Convert.ToDouble(AngleJ2.Text), Convert.ToDouble(AngleJ3.Text), Convert.ToDouble(AngleJ4.Text), 0, 0, 0 };
            double kp = Convert.ToDouble(kpJ4.Text);
            double ki = Convert.ToDouble(kiJ4.Text);
            double kd = Convert.ToDouble(kdJ4.Text);
            controlCommand.holdPWM = holdPWM;
            controlCommand.Angle = Angle;
            controlCommand.kp = kp;
            controlCommand.ki = ki;
            controlCommand.kd = kd;
            controlCommand.joint = 4;
            controlCommand.command = DynamicControl.ControlCommands.SET_PID;
            if (ControlEvent != null)
            {
                ControlEvent(new object(), controlCommand);
            }
        }

        private void SetPIDJ3_Click(object sender, EventArgs e)
        {
            DynamicControl.RobotControlEventArgs controlCommand = new DynamicControl.RobotControlEventArgs();
            int holdPWM = Convert.ToInt32(PWMJ3.Text);
            double[] Angle = new double[USBC.TotalAxes] { Convert.ToDouble(AngleJ0.Text), Convert.ToDouble(AngleJ1.Text), Convert.ToDouble(AngleJ2.Text), Convert.ToDouble(AngleJ3.Text), Convert.ToDouble(AngleJ4.Text), 0, 0, 0 };
            double kp = Convert.ToDouble(kpJ3.Text);
            double ki = Convert.ToDouble(kiJ3.Text);
            double kd = Convert.ToDouble(kdJ3.Text);
            controlCommand.holdPWM = holdPWM;
            controlCommand.Angle = Angle;
            controlCommand.kp = kp;
            controlCommand.ki = ki;
            controlCommand.kd = kd;
            controlCommand.joint = 3;
            controlCommand.command = DynamicControl.ControlCommands.SET_PID;
            if (ControlEvent != null)
            {
                ControlEvent(new object(), controlCommand);
            }

        }

        private void SetPIDJ2_Click(object sender, EventArgs e)
        {
            DynamicControl.RobotControlEventArgs controlCommand = new DynamicControl.RobotControlEventArgs();
            int holdPWM = Convert.ToInt32(PWMJ2.Text);
            double[] Angle = new double[USBC.TotalAxes] { Convert.ToDouble(AngleJ0.Text), Convert.ToDouble(AngleJ1.Text), Convert.ToDouble(AngleJ2.Text), Convert.ToDouble(AngleJ3.Text), Convert.ToDouble(AngleJ4.Text), 0, 0, 0 };
            double kp = Convert.ToDouble(kpJ2.Text);
            double ki = Convert.ToDouble(kiJ2.Text);
            double kd = Convert.ToDouble(kdJ2.Text);
            controlCommand.holdPWM = holdPWM;
            controlCommand.Angle = Angle;
            controlCommand.kp = kp;
            controlCommand.ki = ki;
            controlCommand.kd = kd;
            controlCommand.joint = 2;
            controlCommand.command = DynamicControl.ControlCommands.SET_PID;
            if (ControlEvent != null)
            {
                ControlEvent(new object(), controlCommand);
            }

        }

        private void SetPIDJ1_Click(object sender, EventArgs e)
        {
            DynamicControl.RobotControlEventArgs controlCommand = new DynamicControl.RobotControlEventArgs();
            int holdPWM = Convert.ToInt32(PWMJ1.Text);
            double[] Angle = new double[USBC.TotalAxes] { Convert.ToDouble(AngleJ0.Text), Convert.ToDouble(AngleJ1.Text), Convert.ToDouble(AngleJ2.Text), Convert.ToDouble(AngleJ3.Text), Convert.ToDouble(AngleJ4.Text), 0, 0, 0 };
            double kp = Convert.ToDouble(kpJ1.Text);
            double ki = Convert.ToDouble(kiJ1.Text);
            double kd = Convert.ToDouble(kdJ1.Text);
            controlCommand.holdPWM = holdPWM;
            controlCommand.Angle = Angle;
            controlCommand.kp = kp;
            controlCommand.ki = ki;
            controlCommand.kd = kd;
            controlCommand.joint = 1;
            controlCommand.command = DynamicControl.ControlCommands.SET_PID;
            if (ControlEvent != null)
            {
                ControlEvent(new object(), controlCommand);
            }

        }

        private void SetPIDJ0_Click(object sender, EventArgs e)
        {
            DynamicControl.RobotControlEventArgs controlCommand = new DynamicControl.RobotControlEventArgs();
            int holdPWM = Convert.ToInt32(PWMJ0.Text);
            double[] Angle = new double[USBC.TotalAxes] { Convert.ToDouble(AngleJ0.Text), Convert.ToDouble(AngleJ1.Text), Convert.ToDouble(AngleJ2.Text), Convert.ToDouble(AngleJ3.Text), Convert.ToDouble(AngleJ4.Text), 0, 0, 0 };
            double kp = Convert.ToDouble(kpJ0.Text);
            double ki = Convert.ToDouble(kiJ0.Text);
            double kd = Convert.ToDouble(kdJ0.Text);
            controlCommand.holdPWM = holdPWM;
            controlCommand.Angle = Angle;
            controlCommand.kp = kp;
            controlCommand.ki = ki;
            controlCommand.kd = kd;
            controlCommand.joint = 0;
            controlCommand.command = DynamicControl.ControlCommands.SET_PID;
            if (ControlEvent != null)
            {
                ControlEvent(new object(), controlCommand);
            }

        }

        private void SetPIDAll_Click(object sender, EventArgs e)
        {
            //SetPIDJ0_Click(this, null);
            //SetPIDJ1_Click(this, null);
            //SetPIDJ2_Click(this, null);
            //SetPIDJ3_Click(this, null);
            //SetPIDJ4_Click(this, null);
            

        }

        private void button9_Click(object sender, EventArgs e)
        {
            //try
            //{
            //    picture.Image = portraitor.GetContour();
            //}
            //catch(Exception exp)
            //{
            //    //throw new Exception("Problem in processing image");
            //}


            DynamicControl.RobotControlEventArgs controlCommand = new DynamicControl.RobotControlEventArgs();
            controlCommand.command = DynamicControl.ControlCommands.DRAW;
            if (ControlEvent != null)
            {
                ControlEvent(new object(), controlCommand);
            }

        }

        private void getBackgroundBtn_Click(object sender, EventArgs e)
        {
            try
            {
                picture.Image = portraitor.CaptureBackground();
            }
            catch
            {
                throw new Exception("Problem in getting background image");
            }
        }

        private void threshold1_Scroll(object sender, EventArgs e)
        {
            portraitor.threshold1 = threshold1.Value;
            label2.Text = threshold1.Value.ToString();
        }

        private void threshold2_Scroll(object sender, EventArgs e)
        {
            portraitor.threshold2 = threshold2.Value;
            label2.Text = threshold2.Value.ToString();
        }















        internal void setPicture(Bitmap bitmap)
        {
            this.picture.Image = bitmap;
        }
        public Bitmap getPicture()
        {
            return (Bitmap)this.picture.Image;
        }

    }
}
